/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot2012 extends SimpleRobot {

    // declare variables to use for the different modes during teleoperated
    private static final int MODE_IDLE = 0;
    private static final int MODE_START_LIFTING = 1;
    private static final int MODE_LIFTING = 2;
    private static final int MODE_START_SHOOTING = 3;
    private static final int MODE_SHOOTING = 4;
    private static final int MODE_START_EJECTING = 5;
    private static final int MODE_EJECTING = 6;

    // define the max lifter time and stager time in milliseconds
    private static final double MAX_LIFTER_TIME = 5000;
    private static final double MAX_STAGER_TIME = 5000;

    // declare a variable to hold the current mode
    private static int mode = MODE_IDLE;

    // declare a variable to represent the number of balls we are currently
    // carrying
    private static int numBalls = 0;

    // declare a timer to use for the lifter
    private static Timer lifterTimer = new Timer();

    // declare a timer to use for the stager
    private static Timer stagerTimer = new Timer();
    
    private static LimitSwitch inc = new LimitSwitch(LimitSwitch.PORT_INC);
    private static LimitSwitch dec = new LimitSwitch(LimitSwitch.PORT_DEC);

    public Robot2012() {
    }

    /*
     * This overrides the robotInit code in the SimpleRobot class.
     * It will be called exactly once when the robot is first turned on.
     */
    public void robotInit() {
        Dashboard.setDriveStatus("OK");
        Dashboard.setLifterStatus("OK");
        Dashboard.setShooterStatus("OK");
        Dashboard.setStagerStatus("OK");
        Dashboard.setWedgeStatus("OK");

        // static constructors
        if(DriverStick.init() == false) {
            System.out.println("DriverStick.init failed");
        }
        if(ShooterStick.init() == false) {
            System.out.println("DriverStick.init failed");
        }
        if(Lifter.init() == false) {
            System.out.println("Lifter.init failed");
        }
        if(Stager.init() == false) {
            System.out.println("DriverStick.init failed");
        }
        if(Shooter.init() == false) {
            System.out.println("Shooter.init failed");
        }
        if(Wedge.init() == false) {
            System.out.println("DriverStick.init failed");
        }

        Dashboard.update();
    }

    // returns the maximum time of the power curve for autonomous mode based
    // on how the sliders are set
    public static double getMaxTime() {
        double MAX_DRIVE_TIME = 15.0;
        double sliderValue = DSControls.getSlider(DSControls.SLIDER_AUTONOMOUS_MAX_TIME);

        return sliderValue * MAX_DRIVE_TIME;
    }

    // returns the maximum speed of the power curve for autonomous mode based
    // on how the sliders are set
    public static double getMaxSpeed() {
        double sliderValue = DSControls.getSlider(DSControls.SLIDER_AUTONOMOUS_MAX_SPEED);

        return sliderValue;
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        // get the maximum time for the power curve
        double maxTime = getMaxTime();

        // get the maximum speed for the power curve
        double maxSpeed = getMaxSpeed();

        // define an array that holds the relative speed values from time=0
        // at the beginning of the array and time=maxTime at the end of
        // the array. The array holds "relative speeds" because an entry of 0
        // means 0 speed, and 1 means maxSpeed
        // Arrays in Java are 0 based, so the first entry is at speedPercent[0]
        // and the last entry is at speedPercent[speedPercent.length - 1]
        double speedPercent[] = {0.0, 0.125, 0.4, 0.75, 1.0, 0.75, 0.4, 0.125, 0.0};

        // determine how much time for each interval of the power curve
        // maxTime is the total time
        // the number of intervals is speedPercent.length - 1
        // so the amount of time for each interval is:
        //   intervalTime = maxTime / numIntervals
        //                = maxTime / (speedPercent.length - 1)
        double intervalTime = maxTime / (speedPercent.length - 1);

        //disable watchdog
        getWatchdog().setEnabled(false);

        // create a timer to determine how long we have been running the
        // current interval in the power curve
        Timer timer = new Timer();
        timer.reset();
        timer.start();

        // deploy the wedge
        Wedge.deploy();

        // create a variable to hold the current index in the power curve
        int currentIndex = 0;

        // obtain the initial speed percent from the array, and compute
        // the actual speed by multiplying the speed percent with maxSpeed
        double speed = speedPercent[currentIndex] * maxSpeed;

        // drive forward by the current speed
        Drivetrain.drive(speed, 0);

        // keep driving while we are enabled and in autonomous mode
        while(isAutonomous() && isEnabled()) {
            // check to see if the timer has run longer than the intervalTime
            if(timer.get() > intervalTime) {
                // increment the currentIndex
                currentIndex = currentIndex + 1;

                // if we are past the end of the power curve, break out
                // of the loop
                if(currentIndex >= speedPercent.length) {
                    break;
                }

                // obtain the current speed percent from the array, and compute
                // the actual speed by multiplying the speed percent with maxSpeed
                speed = speedPercent[currentIndex] * maxSpeed;

                // drive forward by the current speed
                Drivetrain.drive(speed, 0);

                // stop the timer, reset it, and start it again
                timer.stop();
                timer.reset();
                timer.start();
            }

            // Sleep for 5 milliseconds to give the cRio a chance to
            // process other events
            Timer.delay(0.005);
        }

        // stop the robot
        Drivetrain.drive(0, 0);
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        // enable the watchdog
        // the watchdog will automatically disable the robot if something
        // in the software hangs. As long as you feed the watchdog, it
        // will assume everything is ok. However, if you go too long
        // without feeding the watchdog, it will assume the software is
        // hung is disable the robot
        getWatchdog().setEnabled(true);

        // loop over the following instructions as long as the robot
        // is enabled and the mode is set to teleoperated (operator control)
        while (isEnabled() && isOperatorControl()) {
            // always feed the watchdog first to let it know everything is ok
            getWatchdog().feed();

            // update the Drivetrain
            Drivetrain.teleopUpdate();

            // update the Wedge
            Wedge.teleopUpdate();

            // update the rest of the robot based on a state machine
            runStateMachine();

            // update the Dashboard
            Dashboard.teleopUpdate();

            // Sleep for 5 milliseconds to give the cRio a chance to
            // Process other events
            Timer.delay(.005);
        }
    }

    /*
     * runStateMachine is called repeatedly during teleoperated mode to
     * update several components of the robot based on a state machine
     */
    private static void runStateMachine() {
        switch (mode) {
            case MODE_IDLE:
                // check to see if we should run the lifter or stop it
                if (numBalls >= 3) {
                    // stop the lifter
                    Lifter.setSpeed(0);
                }
                else {
                    // run the lifter
                    Lifter.setSpeed(1);

                    // check to see if we should lift a ball
                    // Note this code is only called when the
                    // lifter is running
                    if (inc.isRisingEdge()) {
                        mode = MODE_START_LIFTING;
                        break;
                    }
                }

                // check to see if we should start shooting
                if (ShooterStick.shootRisingEdge() ||
                    (ShooterStick.shootPressed() && ShooterStick.fullAutoPressed())) {
                    mode = MODE_START_SHOOTING;
                    break;
                }

                // check to see if we should start ejecting
                if (ShooterStick.reverseRisingEdge() ||
                    (ShooterStick.reversePressed() && ShooterStick.fullAutoPressed())) {
                    mode = MODE_START_EJECTING;
                    break;
                }
                break;
            case MODE_START_LIFTING:
                startLifting();
                break;
            case MODE_LIFTING:
                updateLifting();
                break;
            case MODE_START_SHOOTING:
                startShooting();
                break;
            case MODE_SHOOTING:
                updateShooting();
                break;
            case MODE_START_EJECTING:
                startEjecting();
                break;
            case MODE_EJECTING:
                updateEjecting();
                break;
        } // end of switch (any break above jumps to here)
    } // end of "while enabled"

    /*
     * startLifting is called to initiate the process of lifting a ball
     * into the robot
     */
    private static void startLifting() {
        // start the lifter
        lifterTimer.reset();
        lifterTimer.start();
        Lifter.setSpeed(1);

        // start the stager
        stagerTimer.reset();
        stagerTimer.start();
        Stager.setSpeed(1);

        // change mode to LIFTING
        mode = MODE_LIFTING;
    }


    /*
     * updateLifting is called to repeatedly while the robot is lifting
     * a ball to determine when to stop
     */
    private static void updateLifting() {
        // define the lifter and stager duration in milliseconds
        double lifterDuration = DSControls.getSlider(DSControls.SLIDER_LIFTER_TIMER_PERCENT) * MAX_LIFTER_TIME;
        double stagerDuration = DSControls.getSlider(DSControls.SLIDER_STAGER_TIMER_PERCENT) * MAX_STAGER_TIME;

        // if the lifter has been running more than the lifter duration, stop the lifter
        if (Lifter.isRunning() && lifterTimer.get() > lifterDuration) {
            Lifter.setSpeed(0);
            lifterTimer.stop();
        }

        // if the stager has been running more than the stager duration, stop the stager
        if (Stager.isRunning() && stagerTimer.get() > stagerDuration) {
            Stager.setSpeed(0);
            stagerTimer.stop();
        }

        // if both timers have expired, then increment the ball count and set the mode
        // back to idle
        if (Lifter.isRunning() == false && Stager.isRunning() == false) {
            numBalls = numBalls + 1;

            //If the stager timer goes past stagerDuration then the stager will stop
            //and the stager timer will stop.
            mode = MODE_IDLE;
        }
    }

    private static void startShooting() {
        // stop the lifter
        Lifter.setSpeed(0);

        // start the stager
        Stager.setSpeed(1);

        mode = MODE_SHOOTING;
    }

    private static void updateShooting() {
        boolean shootingComplete = false;

        // check to see if the shoot button is still pressed
        if(ShooterStick.shootPressed()) {
            // if the decrement counter sees a falling edge, it means
            // a ball has been shot. In that case, decrement the number
            // of balls and stop shooting if we are not in full auto mode
            if(dec.isFallingEdge()) {
                numBalls = numBalls - 1;
                if(ShooterStick.fullAutoPressed() == false) {
                    shootingComplete = true;
                }
            }
        } else {
            // set shooting complete to true because the shoot button
            // is no longer pressed
            shootingComplete = true;
        }

        // if shooting is complete, stop the stager and return
        // to idle mode
        if(shootingComplete) {
            Stager.setSpeed(0);
            mode = MODE_IDLE;
        }
    }

    private static void startEjecting() {
        // run the lifter and the stager in reverse
        Lifter.setSpeed(-1);
        Stager.setSpeed(-1);
        mode = MODE_EJECTING;
    }

    private static void updateEjecting() {
        boolean ejectingComplete = false;

        // check to see if the eject button is still pressed
        if(ShooterStick.reversePressed()) {
            // if the increment counter sees a falling edge, it means
            // a ball has been ejected. In that case, decrement the number
            // of balls and stop shooting if we are not in full auto mode
            if(inc.isFallingEdge()) {
                numBalls = numBalls - 1;
                if(ShooterStick.fullAutoPressed() == false) {
                    ejectingComplete = true;
                }
            }
        } else {
            // set ejecting complete to true because the reverse button
            // is no longer pressed
            ejectingComplete = true;
        }

        // if ejecting is complete, stop the lifter and stager and return
        // to idle mode
        if(ejectingComplete) {
            Lifter.setSpeed(0);
            Stager.setSpeed(0);
            mode = MODE_IDLE;
        }
    }
}
